#pragma once
#include <iostream>
#include <cmath>
#include <vector>
#include <string>
#include "getPath.hpp"
#include <nav_msgs/Path.h>
#include <geometry_msgs/PoseStamped.h>
#include <visualization_msgs/Marker.h>
#include <ackermann_msgs/AckermannDriveStamped.h>

class Stanley
{
public:
	Stanley(ros::NodeHandle& nh_, std::vector<State>& path_);
	~Stanley();
	void loop();
	void loop(State& veh_state);
	bool isArrived();

private:
	double stanley_control();
	double normalizeAngle(double angle);
	int calTargetIndex(double f_x, double f_y);
	void updateState(double phi);
	void visualization();

private:
	ros::NodeHandle nh;
	std::vector<State> path;
	double L; // 轴距
	double v;	// 期望速度
	int curr_dir; // 前进 1、后退 -1
	double dt;
	double k; // 系数
	double max_steer;
	int history_ind = 0;
	State curr_state;

	// Rviz可视化相关量
	nav_msgs::Path ref_path;
	nav_msgs::Path track_path;
	visualization_msgs::Marker veh_shape;
	ros::Publisher ref_path_pub;
	ros::Publisher track_path_pub;
	ros::Publisher vehicle_shape_pub;

	// 实物小车控制指令发布器
	ros::Publisher cmd_pub;
};

Stanley::Stanley(ros::NodeHandle& nh_, std::vector<State>& path_) : nh(nh_), path(path_)
{
	nh.getParam("L", L);
	nh.getParam("v", v);
	nh.getParam("k", k);
	nh.getParam("dt", dt);
	nh.getParam("max_steer", max_steer);

	ref_path_pub = nh.advertise<nav_msgs::Path>("ref_path", 10);
	track_path_pub = nh.advertise<nav_msgs::Path>("track_path", 10);
	vehicle_shape_pub = nh.advertise<visualization_msgs::Marker>("veh_shape", 10);
	cmd_pub = nh.advertise<ackermann_msgs::AckermannDriveStamped>("/robot_4/robot_4ackermann_cmd", 10);

	ref_path.header.stamp = ros::Time::now();
	ref_path.header.frame_id = "world";
	int i = 0;
	for (const auto& s : path)
	{
		geometry_msgs::PoseStamped current_pose;
		current_pose.header.seq = i;
		current_pose.header.frame_id = "world";
		current_pose.pose.position.x = s.x;
		current_pose.pose.position.y = s.y;
		current_pose.pose.position.z = 0;

		ref_path.poses.push_back(current_pose);
		i++;
	}
	track_path.header.stamp = ros::Time::now();
	track_path.header.frame_id = "world";

	veh_shape.header.stamp = ros::Time::now();
	veh_shape.header.frame_id = "world";
	veh_shape.ns = "veh_shape";
	veh_shape.id = 0;
	veh_shape.action = visualization_msgs::Marker::ARROW;
	veh_shape.type = visualization_msgs::Marker::ARROW;
	veh_shape.scale.x = 0.02;
	veh_shape.scale.y = 0.04;
	veh_shape.scale.z = 0.05;
	veh_shape.color.a = 0.3;
	veh_shape.color.r = 0;
	veh_shape.color.g = 1;
	veh_shape.color.b = 1;
	veh_shape.pose.orientation.x = 0.0;
	veh_shape.pose.orientation.y = 0.0;
	veh_shape.pose.orientation.z = 0.0;
	veh_shape.pose.orientation.w = 1.0;

	curr_state = path.front();
}

Stanley::~Stanley() {}

void Stanley::loop()
{
	std::cout << "----------------------------" << std::endl;
	std::cout << "当前状态量: " << "["
			<< curr_state.x << ", "
			<< curr_state.y << ", "
			<< curr_state.yaw << ", "
			<< curr_state.dir << "]" << std::endl;
	double phi = stanley_control();
	std::cout << "当前控制量: " << "[" << v * curr_dir << ", " << phi << "]" << std::endl;
	visualization();
	updateState(phi);
}

void Stanley::loop(State& veh_state)
{
	// veh_state.x += L * cos(veh_state.yaw);
	// veh_state.y += L * sin(veh_state.yaw);
	curr_state = veh_state;
	std::cout << "----------------------------" << std::endl;
	std::cout << "当前状态量: " << "["
			<< curr_state.x << ", "
			<< curr_state.y << ", "
			<< curr_state.yaw << ", "
			<< curr_state.dir << "]" << std::endl;
	double phi = stanley_control();
	ackermann_msgs::AckermannDriveStamped cmd_msg;
	cmd_msg.drive.speed = v * curr_dir;
	cmd_msg.drive.steering_angle = phi;
	cmd_pub.publish(cmd_msg);
	visualization();
}

double Stanley::stanley_control()
{
	// 前轮中心点坐标
	// double f_x = curr_state.x + L * cos(curr_state.yaw);
	// double f_y = curr_state.y + L * sin(curr_state.yaw);
	int curr_target_ind = calTargetIndex(curr_state.x, curr_state.y);
	double e_y = sqrt(pow(curr_state.x - path[curr_target_ind].x, 2) + pow(curr_state.y - path[curr_target_ind].y, 2));
	if (atan2((curr_state.y - path[curr_target_ind].y), (curr_state.x - path[curr_target_ind].x)) - curr_state.yaw > 0)
	// if ((curr_state.x - path[curr_target_ind].x) * path[curr_target_ind].yaw - (curr_state.y - path[curr_target_ind].y) < 0)
		e_y = -e_y;
	double theta_e = normalizeAngle(path[curr_target_ind].yaw - curr_state.yaw) * curr_dir;
	double delta_e = atan2(k * e_y, abs(v)); // 返回的是弧度

	double phi = theta_e + delta_e;
	if (phi > max_steer)
		phi = max_steer;
	else if (phi < -max_steer)
		phi = -max_steer;
	return phi;
}

bool Stanley::isArrived()
{
	if (history_ind >= path.size() - 1)
		return true;
	return false;
}

double Stanley::normalizeAngle(double angle)
{
	while (angle > M_PI)
	{
		angle -= 2.0 * M_PI;
	}
	while (angle < -M_PI)
	{
		angle += 2.0 * M_PI;
	}
	return angle;
}

int Stanley::calTargetIndex(double f_x, double f_y)
{
	int index = history_ind; // 求 离当前车辆最近的点的索引，且该索引大于等于历史索引
	double min_dist = INT_MAX;
	for (int i = history_ind; i < path.size(); i++)
	{
		double distance = pow(f_x - path[i].x, 2) + pow(f_y - path[i].y, 2);
		if (distance < min_dist)
		{
			index = i;
			min_dist = distance;
		}
	}
	history_ind = index;
	curr_dir = path[index].dir;
	return index;
}

void Stanley::updateState(double phi)
{
	curr_state.x += v * curr_dir * cos(curr_state.yaw) * dt;
	curr_state.y += v * curr_dir * sin(curr_state.yaw) * dt;
	curr_state.yaw = normalizeAngle(curr_state.yaw + v * curr_dir / L * tan(phi) * dt);
}

void Stanley::visualization()
{
	// 参考路径
	ref_path_pub.publish(ref_path);
	// 实时跟踪路径
	geometry_msgs::PoseStamped p;
	p.pose.position.x = curr_state.x;
	p.pose.position.y = curr_state.y;
	p.pose.position.z = 0;
	track_path.poses.push_back(p);
	track_path_pub.publish(track_path);

	// 显示车辆
	geometry_msgs::Point p1, p2;
	p1.x = curr_state.x;
	p1.y = curr_state.y;
	p1.z = 0;
	double l = 0.1;
	p2.x = curr_state.x + l * cos(curr_state.yaw);
	p2.y = curr_state.y + l * sin(curr_state.yaw);
	p2.z = 0;
	veh_shape.points.clear();
	veh_shape.points.push_back(p1);
	veh_shape.points.push_back(p2);
	vehicle_shape_pub.publish(veh_shape);
}